/*
 * TITLE:	Client.cc
 *
 * PURPOSE:	This file encapsulates the radio/vision client interfaces that 
 *              hide the socket interface to the vision/radio servers
 *
 * WRITTEN BY:  James R Bruce, Scott Lenser, Michael Bowling, Brett Browning
 */
/* LICENSE: */

#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>

#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>

#include "util.h"
#include "constants.h"
#include "utils/socket.h"
#include "reality/net_vision.h"

#include "client.h"


bool Client::initialize(char *hostname, int vport, int rport)
{
  mzero(rcmd);
  rcmd.msg_type = RobotCommandMsgId;

  if (vision_s.connect_client(hostname, vport) != Socket::Client)
    return (false);
  // if (radio_s.connect_client(hostname, rport) != Socket::Client)
  //   return (false);
  return (true);
}

void Client::close()
{
  vision_s.disconnect();
  //radio_s.disconnect();
}

bool Client::configure(VisionConfigMsg &msg)
{
  msg.msg_type = VisionConfigMsgId;
  msg.msg_len = sizeof(msg);

  vision_s.send(&msg, sizeof(msg));
  return(true);
}

bool Client::configure(VisionConfig &vc)
{
  VisionConfigMsg msg;

  msg.msg_type = VisionConfigMsgId;
  msg.msg_len = sizeof(msg);
  msg.config = vc;

  vision_s.send(&msg, sizeof(msg));
  return(true);
}

bool Client::getUpdate(net_vframe &vf)
{
  static char msg[net_vision_out_maxsize];

  if(vision_s.get_status() != Socket::Client) return(false);

  while(true){
    // block until we get a message
    if(vision_s.recv(msg, net_vision_out_maxsize) <= 0) return(false);

    // if it's the right type, copy it and return
    if(((NetMsg*)&msg)->msg_type == VisionFrameMsgId){
      memcpy(&vf,msg,sizeof(vf));
      return(true);
    }
  }
}

bool Client::sendRef(char state) 
{
  SimRefMsg msg;

  mzero(msg);
  msg.msg_type  = SimRefMsgId;
  msg.msg_len   = sizeof(msg);
  msg.ref_state = state;

  if(!vision_s.ready_for_send()){
    fprintf(stderr, "Cannot send yet\n");
    return(false);
  }

  vision_s.send(&msg,sizeof(msg));
  return(true);
}

bool Client::moveBall(double x, double y, double vx, double vy) 
{
  SimMoveBallMsg msg;

  mzero(msg);
  msg.msg_type = SimMoveBallMsgId;
  msg.msg_len  = sizeof(msg);
  msg.loc.set(x,y);
  msg.vel.set(vx,vy);

  vision_s.send(&msg,sizeof(msg));
  return(true);
}

bool Client::moveRobot(uchar team, uchar id, double x, double y, double angle)
{
  SimMoveRobotMsg msg;

  mzero(msg);
  msg.msg_type = SimMoveRobotMsgId;
  msg.msg_len  = sizeof(msg);
  msg.loc.set(x,y);
  msg.angle = angle;
  msg.team = team;
  msg.id = id;

  vision_s.send(&msg,sizeof(msg));
  return(true);
}

bool Client::enableRadio(bool en)
{
  RadioControlMsg msg;

  mzero(msg);
  msg.msg_type = RadioControlMsgId;
  msg.msg_len  = sizeof(msg);
  msg.control  = (en)? VCR_PLAY : VCR_PAUSE;

  vision_s.send(&msg,sizeof(msg));
  return(true);
}

bool Client::radioControl(char ctrl)
{
  RadioControlMsg msg;

  mzero(msg);
  msg.msg_type = RadioControlMsgId;
  msg.msg_len  = sizeof(msg);
  msg.control  = ctrl;

  vision_s.send(&msg,sizeof(msg));
  return(true);
}

bool Client::setCommand(RobotCommand &cmd)
{
  if(num_rcmd_robots >= MAX_TEAM_ROBOTS){
    fprintf(stderr, "ERROR: Cannot add any more commands.\n");
    return(false);
  }

  rcmd.cmd[num_rcmd_robots] = cmd;
  num_rcmd_robots++;
  return(true);
}

bool Client::send(int team)
// sends all the queued commands
{
  // check if there is anything to send
  if(num_rcmd_robots == 0) return(true);

  if(vision_s.get_status() != Socket::Client){
    fprintf(stderr, "ERROR: Socket disconnected so cannot send.\n");
    return(false);
  }

  rcmd.msg_type = RobotCommandMsgId;
  rcmd.msg_len  = rcmd.size(num_rcmd_robots);
  rcmd.team = team;
  rcmd.num_cmds = num_rcmd_robots;
  bool rval = vision_s.send(&rcmd,rcmd.size(num_rcmd_robots));

  // clear the command count so we can add things again
  num_rcmd_robots = 0;

  return(rval);
}
